/*
 * =====================================================================================
 *
 *       Filename:  Acrobat.cpp
 *
 *    Description:  Balansing robot
 *
 *        Version:  1.0
 *        Created:  06/03/2011 08:33:33 PM
 *       Compiler:  gcc
 *
 *         Author:  Toth Tamas (Osiris), osiris996@gamil.com
 *
 * =====================================================================================
 */


#include "pal/palFactory.h"
#include "pal/ConfigStatic.h"
#include "elementconfig.h"
#include "Acrobat.h"

using namespace robots;


Acrobat::Acrobat(){

}

Acrobat::~Acrobat()
{
	delete[] cGeometry;
	delete[] gGeometry;
}

int Acrobat::getElementCount()
{
	return 6;
}

ElementData Acrobat::getElementData(int index)
{
	palMatrix4x4 loc = cGeometry[index]->GetLocationMatrix();
	gGeometry[index]->setTransformationMatrix(loc._mat);
	return (*gGeometry[index]);
}

void Acrobat::move(float x, float y, float z) { }

void Acrobat::command(int commandID, int n, void* args )
{
  float *v= (float*) args;
	cGeometry[0]->SetActive(true);
	switch (commandID)
	{
	case 0:
    pm->SetVoltage(v[0]);
    pm->Apply();
    break;
  default:

		break;
	}
}

void Acrobat::setPosition(float x, float y, float z)
{
	position.x = x;
	position.y = y;
	position.z = z;
}

palVector3 Acrobat::getPosition()
{
	palVector3 pos;
	pos.x = position.x;
	pos.y = position.y;
	pos.z = position.z;
	return pos;
}

void Acrobat::createPhysics()
{
	palBox *b1 = PF->CreateBox();
  b1->Init(position.x,position.y+5,position.z,1.7,0.5,1.3,20);
	cGeometry[0] = b1;
	ElementData *bodyElement = new ElementData(BoxType);
	float* edata = new float[3];
	edata[0] = 1.7;
	edata[1] = 0.5;
	edata[2] = 1.3;
	bodyElement->setData(3, edata);
	bodyElement->setColor(0,200,200);
	palMatrix4x4 loc = b1->GetLocationMatrix();
	bodyElement->setTransformationMatrix(loc._mat);
	gGeometry[0] = bodyElement;
  delete[] edata;

	palSphere *wheel1 = PF->CreateSphere();
	wheel1->Init(position.x, position.y+1, position.z+1, 0.5, 10);
	cGeometry[1] = wheel1;
	ElementData *weelElement = new ElementData(SphereType);
	edata = new float[1];
	edata[0] = 0.5;
	weelElement->setData(1, edata);
	delete[] edata;
	loc = wheel1->GetLocationMatrix();
	bodyElement->setColor(0,0,0);
	weelElement->setTransformationMatrix(loc._mat);
	gGeometry[1] = weelElement;

	palSphere *mid = PF->CreateSphere();
	mid->Init(position.x , position.y+1 , position.z , 0.1, 1);
	cGeometry[2] = mid;
	weelElement = new ElementData(SphereType);
	edata = new float[1];
	edata[0] = 0.1;
	weelElement->setData(1, edata);
	delete[] edata;
	loc = mid->GetLocationMatrix();
	bodyElement->setColor(0,0,0);
	weelElement->setTransformationMatrix(loc._mat);
	gGeometry[2] = weelElement;

	palSphere *wheel2 = PF->CreateSphere();
	wheel2->Init(position.x , position.y +1, position.z-1 , 0.5, 10);
	cGeometry[3] = wheel2;
	weelElement = new ElementData(SphereType);
	edata = new float[1];
	edata[0] = 0.5;
	weelElement->setData(1, edata);
	delete[] edata;
	loc = wheel2->GetLocationMatrix();
	bodyElement->setColor(0,0,0);
	weelElement->setTransformationMatrix(loc._mat);
	gGeometry[3] = weelElement;

	palBox *b2 = PF->CreateBox();
  b2->Init(position.x,position.y+5.4,position.z,6,0.3,0.5,30);
	cGeometry[4] = b2;
	bodyElement = new ElementData(BoxType);
  edata = new float[3];
	edata[0] = 6;
	edata[1] = 0.3;
	edata[2] = 0.5;
	bodyElement->setData(3, edata);
	loc = b2->GetLocationMatrix();
	bodyElement->setColor(200,200,0);
	bodyElement->setTransformationMatrix(loc._mat);
	gGeometry[4] = bodyElement;
	delete[] edata;

	palBox *b3 = PF->CreateBox();
  b3->Init(position.x,position.y+2.9,position.z,0.2,3.5,0.2,20);
	cGeometry[5] = b3;
	bodyElement = new ElementData(BoxType);
	edata = new float[3];
	edata[0] = 0.2;
	edata[1] = 3.5;
	edata[2] = 0.2;
	bodyElement->setData(3, edata);
	loc = b3->GetLocationMatrix();
	bodyElement->setColor(200,200,0);
	bodyElement->setTransformationMatrix(loc._mat);
	gGeometry[5] = bodyElement;
	delete[] edata;



  palRevoluteLink *l1;
  l1 = dynamic_cast<palRevoluteLink *>(PF->CreateObject("palRevoluteLink")); 
  l1->Init(mid,wheel1,position.x,position.y+1,position.z+1,0,0,1);
  l1->SetLimits(0,0);
  palSphere *wheel4 = PF->CreateSphere();

  palRevoluteLink *l2;
  l2 = dynamic_cast<palRevoluteLink *>(PF->CreateObject("palRevoluteLink"));
  l2->Init(mid,wheel2,position.x,position.y+1,position.z-1,0,0,1);
  l2->SetLimits(0,0);

  palRevoluteLink *l3;
  l3 = dynamic_cast<palRevoluteLink *>(PF->CreateObject("palRevoluteLink"));
  l3->Init(b1,mid,position.x,position.y+1,position.z,0,0,1);

  palRevoluteLink *l4;
  l4 = dynamic_cast<palRevoluteLink *>(PF->CreateObject("palRevoluteLink"));
  l4->Init(b1,b2,position.x,position.y+5,position.z,0,0,1);
  l4->SetLimits(0,0);

  palRevoluteLink *l5;
  l5 = dynamic_cast<palRevoluteLink *>(PF->CreateObject("palRevoluteLink"));
  l5->Init(b1,b3,position.x,position.y+5,position.z,0,0,1);
  l5->SetLimits(0,0);

  pm =dynamic_cast<palDCMotor *>(PF->CreateObject("palDCMotor"));
  pm->Init(l3,1000,3.4,10);
  pm->SetVoltage(0);
  pm->Apply();


  }

